/**
 * @FilePath     : /src/lgmg_robots/agv_navigation/agv_navigate/include/agvWorld/bezier.h
 * @Description  : Bezier曲线类的定义，包含生成Bezier曲线、计算曲线长度、导数、曲率等功能。 
 *                 提供了对Bezier曲线的关键点、切线角度和其他相关属性的访问。 
 * @Author       : xiujun.yang
 * @Version      : 1.0.0
 * @LastEditors  : haibo haibo.yang@lgmgim.cn
 * @LastEditTime : 2024-12-19 15:45:28
 * @Copyright (c) 2024 by 临工智能信息科技有限公司, All Rights Reserved. 
**/

#pragma once
#include "geometry.h"
constexpr int CURVE_SEGMENT_COUNT = 100;
///////////////////////////////////////////////////////////////////////////////
struct Derivative {
    float dx;
    float dy;
    Derivative() : dx(0.0f), dy(0.0f) {}
};

struct SegmentData {
    bool exist;
    float t;
    float len;
    SegmentData() : exist(false), t(0.0f), len(0.0f) {}
};

///////////////////////////////////////////////////////////////////////////////
//   Define Bezier curves.
class Bezier
{
 public:
	// bezier constructor
	Bezier(Posture& start_posture, Posture& end_posture, float len1, float len2);
    
	// bezier constructor
    Bezier(const Point& start_point, const Point& ctrl_point1, const Point& ctrl_point2, const Point& end_point);
    
    // bezier constructor
	Bezier() {}

    virtual ~Bezier() {}

	// Generate Bezier curves
    void Create();

	// Get the start angle of the trajectory
    Angle& StartAngle() { return start_angle_; }

	// Get the end angle of the trajectory
    Angle& EndAngle() { return end_angle_; }

	// Get the start point of the trajectory
    Point& StartPoint() { return key_points_[0]; }

	// Get first control point
    const Point& FirstControlPoint() const { return key_points_[1]; }

	// Get second control point
    const Point& SecondControlPoint() const { return key_points_[2]; }

	// Get the end point of the trajectory
    Point& EndPoint() { return key_points_[3]; }

	// Get the length of the trajectory
    float ComputeTotalLength(int segment = -1);

	// Get the length of a segment
    float ComputeCurveLength(float t1, float t2, int segment = -1);

	// Get relative length
    float ComputeRelativeLength(float t);

	// Get the absolute length
    float ComputeAbsoluteLength(float t);

	// Get the reference point
    float ComputeReferencePoint(float t, float x0, float y0);

	// get the point on the curve
    Point& ComputePoint(float t);

	// get the first derivative of the curve
    Derivative& FirstDerivative(float t);

	// get the second derivative of the curve
    Derivative& SecondDerivative(float t);

	// Get the curvature of the curve
    float ComputeCurvature(float t);

	// Set the current lambda
    virtual void SetCurrentLambda(float t, bool update = true);

	// Get the trajectory function
    virtual Point& TrajectoryFunction() { return trajectory_point_; }

	// Get the tangent function
    virtual Angle& TangentFunction() { return tangent_angle_; }

	// Get the curvature function
    virtual float CurvatureFunction() { return curvature_; }

	// Get the radius of the trajectory
    virtual float CurrentRadius() { return current_radius_; }

	// Get radius of the trajectory
    float Radius() const { return radius_; }

	// Get the length of the trajectory
    float Length() const { return total_length_; }

	// Get the length of a segment
    float CurveLength() const { return curve_length_; }

	// Set the length of a segment
    void SetCurveLength(float len) { curve_length_ = len; }

	// Set current t
    void SetCurrentT(float t) { current_t_ = t; }

	// Set last t
    void SetLastT(float t) { last_t_ = t; }

	// Set delta t
    void SetDeltaT(float dt) { delta_t_ = dt; }

	// Get current t
    float GetCurrentT() const { return current_t_; }

	// Get last t
    float GetLastT() const { return last_t_; }

	// Get delta t
    float GetDeltaT() const { return delta_t_; }
 private:
    Derivative first_derivative_;  //一阶导数
    Derivative second_derivative_; //二阶导数
    float len1_;
    float len2_;
    float len3_;
    std::array<Point, 4> key_points_; // 关键点
    Point trajectory_point_;		  // 轨迹点
    Angle tangent_angle_;			  // 切线角
    float curvature_;				  // 曲率
    float radius_;					  // 半径
    float current_radius_;            // 半径
    float total_length_;              // 长度
    float curve_length_;              // 曲线长度
    float current_t_;                
    float last_t_;
    float delta_t_;					  // 增量
    float polygon_length_;		      // 折线长度
    Angle start_angle_;			      // 起始角
    Angle end_angle_;				  // 终止角
    std::array<SegmentData, CURVE_SEGMENT_COUNT> segment_data_;  // 曲线分段数据	
};